#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "fisheye.hpp"
#include "point2loc.hpp"

using namespace std;
using namespace cv;


void xyVision::point2loc(const float& u, const float& v, const float& Z, const string& camconf, float& X, float& Y)
{
	// load camera parameters
	cv::FileStorage fs(camconf, cv::FileStorage::READ);
	Mat temp;
	fs["camera_matrix"] >> temp;
	Matx33d K = Matx33d(temp);
	
	fs["distortion_coefficients"] >> temp;
	Matx41d D = Matx41d(temp);
	//fs["size"] >> imgSz;
	
	Mat ip(1, 1, CV_32FC2);
	Mat pp;
	ip.at<Point2f>(0) = Point2f(u, v);
	xyVision::fisheye::undistortPoints(ip, pp, K, D);

	X = pp.at<Point2f>(0).x * Z;
	Y = pp.at<Point2f>(0).y * Z;
}
